#include "bsp_control.h"
#include "bsp_infrared.h"
#include "bsp_motor.h"
#include "bsp_pid.h"

#define Base_PwmL	500
#define Base_PwmR 550



//	-5		-1		0		1		5
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
  if(htim == &htim2)
  {
    bsp_Infrared_Read();
//    float output=line_pid_realize(&pid_line, Line_Num);
//		MotorOutput(Base_PwmL-(int)output,Base_PwmR+(int)output);
		//MotorOutput(Base_PwmL,Base_PwmR);
  }
}

void Main_TasK(void)
{
//	if(Line_Num==0)
//		MotorOutput(Base_PwmL,Base_PwmR);
//	else if(Line_Num==-1)
//		MotorOutput(Base_PwmL-100,Base_PwmR+100);
//	else if(Line_Num==-5)
//		MotorOutput(Base_PwmL-300,Base_PwmR+300);
//	else if(Line_Num==1)
//		MotorOutput(Base_PwmL+100,Base_PwmR-100);
//	else if(Line_Num==5)
//		MotorOutput(Base_PwmL+300,Base_PwmR-300);
	
	MotorOutput(Base_PwmL,Base_PwmR);
	HAL_Delay(1000);
	MotorOutput(0,0);
}
